# # +--------------------+
# # |     High-Level     |   路径规划（来自SLAM模块）
# # +--------------------+
# # |   Mid-Level Logic  |   控制策略、路径跟踪算法（本模块）
# # +--------------------+
# # |    Low-Level IO    |   与底盘通信、传感器接口、信号输出
# # +--------------------+


# import serial
# import time
# from typing import Tuple, List
# import math

# # ============ 模块 1：状态管理 ============

# # 定义了AGV的当前状态，包含位置、车头方向、规划的路径和目标点
# class AGVState:
#     def __init__(self):
#         self.position: Tuple[float, float] = (0.0, 0.0)
#         self.orientation: float = 0.0  # 航向角，单位度
#         self.path: List[Tuple[float, float]] = []
#         self.goal: Tuple[float, float] = (0.0, 0.0)

# # ============ 模块 2：传感与定位（SLAM模块提供接口）===========

# # 定位： 位置 + 航向角
# class LocalizationModule:
#     def get_position(self) -> Tuple[float, float]:
#         # TODO: 接入SLAM模块定位接口
#         return (1.0, 2.0)

#     def get_orientation(self) -> float:
#         # TODO: 获取航向角
#         return 90.0

# # ============ 模块 3：路径管理 ============

# # 路径规划接口：提供路径序列
# class PathPlannerInterface:
#     def get_path(self, start: Tuple[float, float], goal: Tuple[float, float]) -> List[Tuple[float, float]]:
#         # TODO: 由SLAM或地图模块提供路径序列
#         return [start, goal]

# # ============ 模块 4：控制策略 ============
# import rospy
# from geometry_msgs.msg import Twist
# from typing import Tuple
# class ControlModule:
#     def __init__(self):
#         # 如果你的主程序没有初始化 ROS 节点，可以在这里 uncomment。
#         # 但通常建议在主程序入口初始化一次。
#         # if not rospy.core.is_initialized():
#         #     rospy.init_node('control_receiver_node', anonymous=True)

#         # 用于存储从 /cmd_vel 话题接收到的最新完整的 Twist 消息对象
#         # 初始化为 None，表示尚未收到消息
#         self._latest_twist_command = None

#         # 创建订阅者，订阅 /cmd_vel 话题，消息类型为 Twist
#         self.cmd_vel_subscriber = rospy.Subscriber('/cmd_vel', Twist, self._cmd_vel_callback)

#         rospy.loginfo("ControlModule initialized and subscribed to /cmd_vel topic for Twist messages.")

#     def _cmd_vel_callback(self, msg: Twist):
#         """
#         /cmd_vel 话题的回调函数。
#         接收完整的 Twist 消息，并存储它。
#         """
#         # 直接将接收到的完整的 Twist 消息对象存储到类属性中
#         self._latest_twist_command = msg

#         # # 打印接收到的数据以便调试，这次我们可以选择打印更多信息
#         # # 也可以只确认收到了消息
#         # rospy.loginfo(f"Received cmd_vel update: Linear.x = {msg.linear.x:.3f}, Linear.y = {msg.linear.y:.3f}, Linear.z = {msg.linear.z:.3f}, Angular.x = {msg.angular.x:.3f}, Angular.y = {msg.angular.y:.3f}, Angular.z = {msg.angular.z:.3f}")


#     def get_latest_command(self):
#         """
#         返回从 /cmd_vel 话题接收到的最新完整 Twist 消息对象。
#         如果尚未收到消息，返回 None。
#         """
#         # 直接返回存储的完整 Twist 消息对象
#         return self._latest_twist_command

# # ============ 模块 5：运动执行 (新增接收功能并添加打印信息) ============
# class MotionExecutor:
#     def __init__(self, serial_port='/dev/ttyTHS0', baud_rate=115200):
#         """
#         初始化 MotionExecutor 并设置串口通信。
#         Args:
#             serial_port (str): 要使用的串口号。
#             baud_rate (int): 串口通信的波特率。
#         """
#         self.serial_port = serial_port
#         self.baud_rate = baud_rate
#         self.ser = None
#         try:
#             self.ser = serial.Serial(
#                 port=self.serial_port,
#                 baudrate=self.baud_rate,
#                 bytesize=serial.EIGHTBITS,
#                 parity=serial.PARITY_NONE,
#                 stopbits=serial.STOPBITS_ONE,
#                 timeout=0.01
#             )
#             time.sleep(1)  # 等待串口初始化
#             print(f"[Control] 串口 {self.serial_port} 打开成功。")
#         except serial.SerialException as e:
#             print(f"[Control] 打开串口 {self.serial_port} 错误: {e}")

#     def send(self, speed_mode: str, direction: float):
#         """
#         通过串口发送控制指令（速度和方向）。
#         Args:
#             speed_mode (str): 速度模式 ('stop', 'slow', 'fast')。
#             direction (float): 方向角，单位度。
#         """
#         command = ""
#         if speed_mode == "stop":
#             print(f"[Control] 发送停止指令. Holding direction: {direction:.1f}°")
#             # TODO: 定义停止指令的协议，并与单片机约定一致
#             command = f"STOP,{direction:.1f}\n"
#         else:
#             print(f"[Control] 发送行驶指令. Speed: {speed_mode}, Direction: {direction:.1f}°")
#             # TODO: 定义正常行驶指令的协议，并与单片机约定一致
#             command = f"MOVE,{speed_mode},{direction:.1f}\n"

#         # 检查串口是否打开，并添加相应的打印信息
#         if self.ser and self.ser.isOpen():
#             print(f"[Control] 串口已打开，尝试发送数据...") # 进入 if 块时打印
#             try:
#                 self.ser.write(command.encode())
#                 print(f"[Control] 已发送: {command.strip()}")
#             except serial.SerialException as e:
#                 print(f"[Control] 发送指令错误: {e}")
#         else:
#             print("[Control] 串口未打开或初始化失败，无法发送指令。") # 进入 else 块时打印

#     def receive(self):
#         """
#         从串口读取并处理接收到的数据。
#         """
#         if self.ser and self.ser.isOpen():
#             print(f"[Control] 串口已打开，尝试接收数据...") # 进入 if 块时打印
#             try:
#                 # 检查接收缓冲区是否有数据
#                 if self.ser.in_waiting > 0:
#                     # 读取所有可用数据
#                     received_data_bytes = self.ser.read(self.ser.in_waiting)
#                     received_data_str = received_data_bytes.decode('utf-8', errors='ignore').strip()

#                     if received_data_str:
#                         print(f"[Control] 接收到数据: {received_data_str}")

#                         # TODO: 在这里解析接收到的数据，判断指令是否被收到和执行完成
#                         # 您需要与单片机约定好返回的状态信息格式
#                         # 例如，如果单片机返回 "CMD_RECEIVED" 表示收到指令
#                         # 如果返回 "CMD_DONE" 表示指令执行完成
#                         if received_data_str == "CMD_RECEIVED":
#                             print("[Control] 指令已被单片机接收。")
#                             # 可以根据需要更新 AGV 状态或执行其他逻辑
#                         elif received_data_str == "CMD_DONE":
#                              print("[Control] 指令已在单片机上执行完成。")
#                              # 可以根据需要更新 AGV 状态或执行其他逻辑，例如发送下一个指令
#                         # 添加更多 elif 来处理其他可能的返回状态
#                         else:
#                             print(f"[Control] 接收到未知状态信息: {received_data_str}")

#                     return received_data_str
#                 # else:
#                 #     # 没有数据时不做任何事情
#                 #     pass

#             except serial.SerialException as e:
#                 print(f"[Control] 读取串口数据错误: {e}")
#             except Exception as e:
#                 print(f"[Control] 处理接收数据时发生错误: {e}")
#         else:
#             print("[Control] 串口未打开或初始化失败，无法接收数据。")
#         return None


#     def close(self):
#         """
#         关闭串口。
#         """
#         if self.ser and self.ser.isOpen():
#             self.ser.close()
#             print(f"[Control] 串口 {self.serial_port} 已关闭。")


# # ============ 模块 6：避障模块接口 ============

# class ObstacleAvoidanceModule:
#     def should_stop(self) -> bool:
#         # TODO: Fu 的避障算法结果替换这里
#         return False

# # ============ 修改 AGVController 类 (在主循环中调用接收功能) ============

# class AGVController:
#     def __init__(self):
#         self.state = AGVState()
#         self.localizer = LocalizationModule()
#         self.planner = PathPlannerInterface()
#         self.controller = ControlModule()
#         # self.executor = MotionExecutor()
#         self.executor = MotionExecutor(serial_port='/dev/ttyTHS0', baud_rate=115200)

#         self.obstacle_checker = ObstacleAvoidanceModule()

#     def initialize(self, goal: Tuple[float, float]):
#         self.state.goal = goal
#         self.state.position = self.localizer.get_position()
#         self.state.orientation = self.localizer.get_orientation()
#         self.state.path = self.planner.get_path(self.state.position, self.state.goal)


#     def run(self):
#         print("[AGV] Starting main ROS control loop.")
#         rate = rospy.Rate(10)  # 控制循环频率，例如 10 Hz
#         try:
#             while not rospy.is_shutdown(): # 使用 ROS 的 shutdown 标志来控制循环结束

#                 # 1. 获取从 ROS 话题接收到的最新控制指令
#                 # update_control() 现在不接受参数，直接返回订阅到的速度和方向
#                 msg = self.controller.get_latest_command()
#                 if msg is not None:
#                     # msg 现在是一个 geometry_msgs/Twist 对象
#                     # 提取各个分量
#                     linear_x = msg.linear.x
#                     linear_y = msg.linear.y
#                     linear_z = msg.linear.z
#                     angular_x = msg.angular.x
#                     angular_y = msg.angular.y
#                     angular_z = msg.angular.z
#                     # 格式化字符串（
#                     formatted_output = (
#                         f"Received complete cmd_vel:"
#                         f"\n  Linear Velocity:"
#                         f"\n    x: {linear_x:.3f}"
#                         f"\n    y: {linear_y:.3f}"
#                         f"\n    z: {linear_z:.3f}"
#                         f"\n  Angular Velocity:"
#                         f"\n    x: {angular_x:.3f}"
#                         f"\n    y: {angular_y:.3f}"
#                         f"\n    z: {angular_z:.3f}"
#                     )
#                     # 使用 rospy.loginfo 在 ROS 日志中输出
#                     rospy.loginfo(formatted_output)



#                     # if self.obstacle_checker.should_stop():
#                     #     speed = "stop"  # 避障时强制停止
#                     #     rospy.logwarn("[Obstacle] Detected obstacle, overriding command to stop.")
#                     # else:
#                     #     # 打印接收到的指令（如果避障没有强制停止）
#                     #     rospy.loginfo(f"Using received command: Speed={speed}, Direction={direction}°")


#                     # # 3. 执行控制信号：将接收到的速度和方向发送给运动执行模块
#                     self.executor.send(linear_x, angular_z)


#                     # # 4. 检查并处理串口接收到的数据（来自 MotionExecutor 的反馈）
#                     received_status = self.executor.receive()
#                     # 你可以在这里根据 received_status 来决定下一步的动作，
#                     # 例如根据底盘的反馈调整控制逻辑或更新状态。
#                     if received_status:
#                         rospy.loginfo(f"[AGV] Received status from executor: {received_status}")

#                     # 5. 判断是否到达终点
#                 else:
#                     # 如果 msg 是 None，说明还没有收到任何消息
#                     rospy.loginfo("No cmd_vel message received yet.")                

#                 # 控制循环频率
#                 rate.sleep()

#         except rospy.ROSInterruptException:
#             rospy.loginfo("[AGV] ROS interrupt received. Shutting down.")
#         finally:
#             self.executor.close()
#             rospy.loginfo("[AGV] AGVController shut down.")




# # ============ 入口函数 ============

# if __name__ == "__main__":
#     agv = AGVController()
#     agv.initialize(goal=(5.0, 5.0))
#     agv.run()


# +--------------------+
# |     High-Level     |   路径规划（来自SLAM模块）
# +--------------------+
# |   Mid-Level Logic  |   控制策略、路径跟踪算法（本模块）
# +--------------------+
# |    Low-Level IO    |   与底盘通信、传感器接口、信号输出
# +--------------------+


import serial
import time
from typing import Tuple, List
import math

# ============ 模块 1：状态管理 ============

# 定义了AGV的当前状态，包含位置、车头方向、规划的路径和目标点
class AGVState:
    def __init__(self):
        self.position: Tuple[float, float] = (0.0, 0.0)
        self.orientation: float = 0.0  # 航向角，单位度
        self.path: List[Tuple[float, float]] = []
        self.goal: Tuple[float, float] = (0.0, 0.0)

# ============ 模块 2：传感与定位（SLAM模块提供接口）===========

# 定位： 位置 + 航向角
class LocalizationModule:
    def get_position(self) -> Tuple[float, float]:
        # TODO: 接入SLAM模块定位接口
        return (1.0, 2.0)

    def get_orientation(self) -> float:
        # TODO: 获取航向角
        return 90.0

# ============ 模块 3：路径管理 ============

# 路径规划接口：提供路径序列
class PathPlannerInterface:
    def get_path(self, start: Tuple[float, float], goal: Tuple[float, float]) -> List[Tuple[float, float]]:
        # TODO: 由SLAM或地图模块提供路径序列
        return [start, goal]

# ============ 模块 4：控制策略 ============
import rospy
from geometry_msgs.msg import Twist
from typing import Tuple
class ControlModule:
    def __init__(self):
        # 如果你的主程序没有初始化 ROS 节点，可以在这里 uncomment。
        # 但通常建议在主程序入口初始化一次。
        # if not rospy.core.is_initialized():
        #     rospy.init_node('control_receiver_node', anonymous=True)

        # 用于存储从 /cmd_vel 话题接收到的最新完整的 Twist 消息对象
        # 初始化为 None，表示尚未收到消息
        self._latest_twist_command = None

        # 创建订阅者，订阅 /cmd_vel 话题，消息类型为 Twist
        self.cmd_vel_subscriber = rospy.Subscriber('/cmd_vel', Twist, self._cmd_vel_callback)

        rospy.loginfo("ControlModule initialized and subscribed to /cmd_vel topic for Twist messages.")

    def _cmd_vel_callback(self, msg: Twist):
        """
        /cmd_vel 话题的回调函数。
        接收完整的 Twist 消息，并存储它。
        """
        # 直接将接收到的完整的 Twist 消息对象存储到类属性中
        self._latest_twist_command = msg

        # # 打印接收到的数据以便调试，这次我们可以选择打印更多信息
        # # 也可以只确认收到了消息
        # rospy.loginfo(f"Received cmd_vel update: Linear.x = {msg.linear.x:.3f}, Linear.y = {msg.linear.y:.3f}, Linear.z = {msg.linear.z:.3f}, Angular.x = {msg.angular.x:.3f}, Angular.y = {msg.angular.y:.3f}, Angular.z = {msg.angular.z:.3f}")


    def get_latest_command(self):
        """
        返回从 /cmd_vel 话题接收到的最新完整 Twist 消息对象。
        如果尚未收到消息，返回 None。
        """
        # 直接返回存储的完整 Twist 消息对象
        return self._latest_twist_command

# ============ 模块 5：运动执行 (新增接收功能并添加打印信息) ============
# Mapping of motion commands to 8-bit integer values
motion_commands = {
    "left": 0x80,      # 1000 0000
    "right": 0x40,     # 0100 0000
    "forward": 0x20,   # 0010 0000
    "backward": 0x10,  # 0001 0000
    "break": 0x08,     # 0000 1000 - Not used for now, set to 0
    "lift": 0x04,      # 0000 0100 - Not used for now, set to 0
    "lay": 0x02        # 0000 0010 - Not used for now, set to 0
}

# Include necessary libraries
import serial
import time
import math
from typing import Tuple, List

# Assuming the rest of the classes (AGVState, LocalizationModule, PathPlannerInterface, ControlModule, ObstacleAvoidanceModule, AGVController) remain the same or are adapted elsewhere to provide the motion command string to MotionExecutor.

# ============ 模块 5：运动执行 (新增接收功能并添加打印信息) ============
class MotionExecutor:
    def __init__(self, serial_port='/dev/ttyTHS0', baud_rate=115200):
        """
        初始化 MotionExecutor 并设置串口通信。
        Args:
            serial_port (str): 要使用的串口号。
            baud_rate (int): 串口通信的波特率。
        """
        self.serial_port = serial_port
        self.baud_rate = baud_rate
        self.ser = None
        self.last_sent_command = None # Store the last sent command for verification

        try:
            self.ser = serial.Serial(
                port=self.serial_port,
                baudrate=self.baud_rate,
                bytesize=serial.EIGHTBITS,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
                timeout=0.1 # Increased timeout to allow some time for the microcontroller to respond
            )
            time.sleep(2)  # Wait for the serial port to initialize, increased sleep time
            print(f"[Control] 串口 {self.serial_port} 打开成功。")
        except serial.SerialException as e:
            print(f"[Control] 打开串口 {self.serial_port} 错误: {e}")
        except Exception as e:
            print(f"[Control] 初始化串口时发生未知错误: {e}")


    def send_command(self, command_name: str):
        """
        通过串口发送控制指令: 8-bit byte
        Args:
            command_name (str): 运动指令名称 ('left', 'right', 'forward', 'backward', 'break', 'lift', 'lay').
        """
        if command_name not in motion_commands:
            print(f"[Control] 未知的运动指令: {command_name}")
            return

        # Get the corresponding byte value for the command
        command_byte_int = motion_commands[command_name]
        command_byte = bytes([command_byte_int]) # Convert integer to byte

        # Check if serial port is open
        if self.ser and self.ser.isOpen():
            try:
                self.ser.write(command_byte)
                self.last_sent_command = command_byte # Store the sent command
                print(f"[Control] 已发送指令: {command_name} (Hex: {command_byte.hex()})")
            except serial.SerialException as e:
                print(f"[Control] 发送指令错误: {e}")
            except Exception as e:
                print(f"[Control] 发送数据时发生未知错误: {e}")
        else:
            print("[Control] 串口未打开或初始化失败，无法发送指令。")


    def receive_feedback(self):
        """
        从串口读取并处理接收到的数据，验证是否与最后发送的指令一致。
        """
        if self.ser and self.ser.isOpen():
            try:
                # Check if there's data in the receive buffer
                if self.ser.in_waiting > 0:
                    # Read exactly one byte
                    received_byte = self.ser.read(1)

                    if received_byte:
                        print(f"[Control] 接收到数据: (Hex: {received_byte.hex()})")

                        # Verify if the received byte matches the last sent command
                        if self.last_sent_command is not None and received_byte == self.last_sent_command:
                            print("[Control] 指令执行完成确认。")
                            self.last_sent_command = None # Clear the last sent command after confirmation
                            return True # Indicate successful execution confirmation
                        else:
                            print(f"[Control] 接收到的数据与最后发送的指令不匹配。")
                            return False # Indicate mismatch
                    else:
                         print("[Control] 串口in_waiting > 0 但未读到数据，可能是读取超时。")
                         return False # Nothing received within timeout
                else:
                    print("[Control] 接收缓冲区没有数据。") # Optional: uncomment for debugging if needed
                    return False # No data in buffer

            except serial.SerialException as e:
                print(f"[Control] 读取串口数据错误: {e}")
                return False # Indicate error
            except Exception as e:
                print(f"[Control] 处理接收数据时发生未知错误: {e}")
                return False # Indicate error
        else:
            print("[Control] 串口未打开或初始化失败，无法接收数据。") # Optional: uncomment for debugging if needed
            return False # Indicate serial port not open

    def close(self):
        """
        关闭串口。
        """
        if self.ser and self.ser.isOpen():
            self.ser.close()
            print(f"[Control] 串口 {self.serial_port} 已关闭。")


# ============ 模块 6：避障模块接口 ============

class ObstacleAvoidanceModule:
    def should_stop(self) -> bool:
        # TODO: Fu 的避障算法结果替换这里
        return False

# ============ 修改 AGVController 类 (在主循环中调用接收功能) ============

class AGVController:
    def __init__(self):
        self.state = AGVState()
        self.localizer = LocalizationModule()
        self.planner = PathPlannerInterface()
        self.controller = ControlModule()
        # self.executor = MotionExecutor()
        self.executor = MotionExecutor(serial_port='/dev/ttyTHS0', baud_rate=115200)

        self.obstacle_checker = ObstacleAvoidanceModule()

    def initialize(self, goal: Tuple[float, float]):
        self.state.goal = goal
        self.state.position = self.localizer.get_position()
        self.state.orientation = self.localizer.get_orientation()
        self.state.path = self.planner.get_path(self.state.position, self.state.goal)
    def _determine_motion_command(self, msg):
        """
        根据 ROS Twist 消息确定运动指令名称 (仅限前后左右)。
        Args:
            msg (Twist): 接收到的 ROS Twist 消息对象。
        Returns:
            str or None: 对应的运动指令名称 ('forward', 'backward', 'left', 'right')，
                        如果没有明显的运动指令则返回 None。
        """
        if msg is None:
            return None
        linear_x = msg.linear.x
        angular_z = msg.angular.z

        # 设定阈值，用于判断速度是否足够大到被认为是运动指令
        # 这些阈值可能需要根据你的机器人实际情况进行调整
        linear_threshold = 0.15 # 线速度阈值
        angular_threshold = 0.2  # 角速度阈值

        command = None

        # 优先判断前进或后退
        if linear_x > linear_threshold:
            command = "forward"
        elif linear_x < -linear_threshold:
            command = "backward"
        # 如果前进后退的速度不显著，再判断转向
        # 这里增加了一个条件：只有在线速度接近零时才考虑纯旋转指令
        # elif abs(linear_x) < linear_threshold / 2.0: # 允许一个较小的线速度容忍范围来触发旋转
        else:
            if angular_z > angular_threshold:
                command = "left"
            # elif angular_z < -angular_threshold:
            else:
                command = "right"
        return command


    def run(self):
        print("[AGV] Starting main ROS control loop.")
        rate = rospy.Rate(10)  # 控制循环频率，例如 10 Hz
        try:
            while not rospy.is_shutdown(): # 使用 ROS 的 shutdown 标志来控制循环结束

                # 1. 获取从 ROS 话题接收到的最新控制指令
                # update_control() 现在不接受参数，直接返回订阅到的速度和方向
                msg = self.controller.get_latest_command()
                if msg is not None:
                    # msg 现在是一个 geometry_msgs/Twist 对象
                    # 提取各个分量
                    formatted_output = (
                        f"Received cmd_vel:"
                        f"\n  Linear Velocity:"
                        f"\n    x: {msg.linear.x:.3f}"
                        f"\n    y: {msg.linear.y:.3f}"
                        f"\n    z: {msg.linear.z:.3f}"
                        f"\n  Angular Velocity:"
                        f"\n    x: {msg.angular.x:.3f}"
                        f"\n    y: {msg.angular.y:.3f}"
                        f"\n    z: {msg.angular.z:.3f}"
                    )
                    # 使用 rospy.loginfo 在 ROS 日志中输出
                    rospy.loginfo(formatted_output)
                    # if self.obstacle_checker.should_stop():
                    #     speed = "stop"  # 避障时强制停止
                    #     rospy.logwarn("[Obstacle] Detected obstacle, overriding command to stop.")
                    # else:
                    #     # 打印接收到的指令（如果避障没有强制停止）
                    #     rospy.loginfo(f"Using received command: Speed={speed}, Direction={direction}°")

                    # # 3. 执行控制信号：将接收到的速度和方向发送给运动执行模块
                    command = self._determine_motion_command(msg)
                    # print("test", command)
                    if command:
                        # 发送运动指令
                        self.executor.send_command(command)


                    # # 4. 检查并处理串口接收到的数据（来自 MotionExecutor 的反馈）
                    # received_status = self.executor.receive()
                    self.executor.receive_feedback()
                    # 你可以在这里根据 received_status 来决定下一步的动作，
                    # 例如根据底盘的反馈调整控制逻辑或更新状态。
                    # if received_status:
                    #     rospy.loginfo(f"[AGV] Received status from executor: {received_status}")

                    # 5. 判断是否到达终点
                else:
                    # 如果 msg 是 None，说明还没有收到任何消息
                    rospy.loginfo("No cmd_vel message received yet.")                

                # 控制循环频率
                rate.sleep()

        except rospy.ROSInterruptException:
            rospy.loginfo("[AGV] ROS interrupt received. Shutting down.")
        finally:
            self.executor.close()
            rospy.loginfo("[AGV] AGVController shut down.")




# ============ 入口函数 ============

if __name__ == "__main__":
    agv = AGVController()
    agv.initialize(goal=(5.0, 5.0))
    agv.run()